#define _GNU_SOURCE

#include <errno.h>
#include <math.h>
#include <sched.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include <pthread.h>
#include <stdbool.h>

#include "ecrt.h"

#define PERIOD_NS (1000000)
#define MAX_SAFE_STACK (8 * 1024)
#define NSEC_PER_SEC (1000000000)
#define CLOCK_TO_USE CLOCK_MONOTONIC
#define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

/* EtherCAT slave position and ID */
#define AC1181_POS 0, 0
#define AC1181_ID 0x000002ea, 0xc0041181
#define AL3608_POS 0, 1
#define AL3608_ID 0x000002ea, 0xc0043608

/************************* AC1181结构体 *************************/
typedef struct
{
    ec_slave_config_t *sc_;
    ec_slave_config_state_t sc_state_;
    ec_domain_t *domain;
    ec_domain_state_t domain_state;
    uint8_t *domainpd;
    struct
    {
        unsigned int output[8];
        uint8_t output_bit_position[8];
        unsigned int input[8];
        uint8_t input_bit_position[8];
    } offset;
    struct
    {
        uint8_t output[8];
        uint8_t input[8];
    } data;
} ac1181_info_t;

/************************* AL3608结构体 *************************/
typedef struct
{
    ec_slave_config_t *sc_;
    ec_slave_config_state_t sc_state_;
    ec_domain_t *domain;
    ec_domain_state_t domain_state;
    uint8_t *domainpd;
    unsigned int offset[73];
    uint8_t offset_bit_position[73];

    struct
    {
        uint16_t input[8];
    } data;
} al3608_info_t;

/************************* 全局变量 *************************/
ac1181_info_t ac1181_info = {0};
al3608_info_t al3608_info = {0};
ec_domain_t *domain = NULL;
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

/************************* PDO entry注册 *************************/
const static ec_pdo_entry_reg_t domain_regs[] = {
    // AC1181 8路输入
    {AC1181_POS, AC1181_ID, 0x6000, 0x01, &ac1181_info.offset.input[0], &ac1181_info.offset.input_bit_position[0]},
    {AC1181_POS, AC1181_ID, 0x6010, 0x01, &ac1181_info.offset.input[1], &ac1181_info.offset.input_bit_position[1]},
    {AC1181_POS, AC1181_ID, 0x6020, 0x01, &ac1181_info.offset.input[2], &ac1181_info.offset.input_bit_position[2]},
    {AC1181_POS, AC1181_ID, 0x6030, 0x01, &ac1181_info.offset.input[3], &ac1181_info.offset.input_bit_position[3]},
    {AC1181_POS, AC1181_ID, 0x6040, 0x01, &ac1181_info.offset.input[4], &ac1181_info.offset.input_bit_position[4]},
    {AC1181_POS, AC1181_ID, 0x6050, 0x01, &ac1181_info.offset.input[5], &ac1181_info.offset.input_bit_position[5]},
    {AC1181_POS, AC1181_ID, 0x6060, 0x01, &ac1181_info.offset.input[6], &ac1181_info.offset.input_bit_position[6]},
    {AC1181_POS, AC1181_ID, 0x6070, 0x01, &ac1181_info.offset.input[7], &ac1181_info.offset.input_bit_position[7]},
    // AC1181 8路输出
    {AC1181_POS, AC1181_ID, 0x7000, 0x01, &ac1181_info.offset.output[0], &ac1181_info.offset.output_bit_position[0]},
    {AC1181_POS, AC1181_ID, 0x7010, 0x01, &ac1181_info.offset.output[1], &ac1181_info.offset.output_bit_position[1]},
    {AC1181_POS, AC1181_ID, 0x7020, 0x01, &ac1181_info.offset.output[2], &ac1181_info.offset.output_bit_position[2]},
    {AC1181_POS, AC1181_ID, 0x7030, 0x01, &ac1181_info.offset.output[3], &ac1181_info.offset.output_bit_position[3]},
    {AC1181_POS, AC1181_ID, 0x7040, 0x01, &ac1181_info.offset.output[4], &ac1181_info.offset.output_bit_position[4]},
    {AC1181_POS, AC1181_ID, 0x7050, 0x01, &ac1181_info.offset.output[5], &ac1181_info.offset.output_bit_position[5]},
    {AC1181_POS, AC1181_ID, 0x7060, 0x01, &ac1181_info.offset.output[6], &ac1181_info.offset.output_bit_position[6]},
    {AC1181_POS, AC1181_ID, 0x7070, 0x01, &ac1181_info.offset.output[7], &ac1181_info.offset.output_bit_position[7]},
    // AL3608 8路16位模拟量输入
    // CJCompensation 1~8
    {AL3608_POS, AL3608_ID, 0x7000, 0x01, &al3608_info.offset[0]},
    {AL3608_POS, AL3608_ID, 0x7010, 0x01, &al3608_info.offset[1]},
    {AL3608_POS, AL3608_ID, 0x7020, 0x01, &al3608_info.offset[2]},
    {AL3608_POS, AL3608_ID, 0x7030, 0x01, &al3608_info.offset[3]},
    {AL3608_POS, AL3608_ID, 0x7040, 0x01, &al3608_info.offset[4]},
    {AL3608_POS, AL3608_ID, 0x7050, 0x01, &al3608_info.offset[5]},
    {AL3608_POS, AL3608_ID, 0x7060, 0x01, &al3608_info.offset[6]},
    {AL3608_POS, AL3608_ID, 0x7070, 0x01, &al3608_info.offset[7]},
    // Channel 1
    {AL3608_POS, AL3608_ID, 0x6000, 0x01, &al3608_info.offset[8], &al3608_info.offset_bit_position[8]},
    {AL3608_POS, AL3608_ID, 0x6000, 0x02, &al3608_info.offset[9], &al3608_info.offset_bit_position[9]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL}, // Gap 4bit
    {AL3608_POS, AL3608_ID, 0x6000, 0x07, &al3608_info.offset[11], &al3608_info.offset_bit_position[11]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL}, // Gap 7bit
    {AL3608_POS, AL3608_ID, 0x6000, 0x0f, &al3608_info.offset[13], &al3608_info.offset_bit_position[13]},
    {AL3608_POS, AL3608_ID, 0x6000, 0x10, &al3608_info.offset[14], &al3608_info.offset_bit_position[14]},
    {AL3608_POS, AL3608_ID, 0x6000, 0x11, &al3608_info.offset[15]}, // Value 1
    // Channel 2
    {AL3608_POS, AL3608_ID, 0x6010, 0x01, &al3608_info.offset[16], &al3608_info.offset_bit_position[16]},
    {AL3608_POS, AL3608_ID, 0x6010, 0x02, &al3608_info.offset[17], &al3608_info.offset_bit_position[17]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6010, 0x07, &al3608_info.offset[19], &al3608_info.offset_bit_position[19]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6010, 0x0f, &al3608_info.offset[21], &al3608_info.offset_bit_position[21]},
    {AL3608_POS, AL3608_ID, 0x6010, 0x10, &al3608_info.offset[22], &al3608_info.offset_bit_position[22]},
    {AL3608_POS, AL3608_ID, 0x6010, 0x11, &al3608_info.offset[23]}, // Value 2
    // Channel 3
    {AL3608_POS, AL3608_ID, 0x6020, 0x01, &al3608_info.offset[24], &al3608_info.offset_bit_position[24]},
    {AL3608_POS, AL3608_ID, 0x6020, 0x02, &al3608_info.offset[25], &al3608_info.offset_bit_position[25]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6020, 0x07, &al3608_info.offset[27], &al3608_info.offset_bit_position[27]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6020, 0x0f, &al3608_info.offset[29], &al3608_info.offset_bit_position[29]},
    {AL3608_POS, AL3608_ID, 0x6020, 0x10, &al3608_info.offset[30], &al3608_info.offset_bit_position[30]},
    {AL3608_POS, AL3608_ID, 0x6020, 0x11, &al3608_info.offset[31]}, // Value 3
    // Channel 4
    {AL3608_POS, AL3608_ID, 0x6030, 0x01, &al3608_info.offset[32], &al3608_info.offset_bit_position[32]},
    {AL3608_POS, AL3608_ID, 0x6030, 0x02, &al3608_info.offset[33], &al3608_info.offset_bit_position[33]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6030, 0x07, &al3608_info.offset[35], &al3608_info.offset_bit_position[35]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6030, 0x0f, &al3608_info.offset[37], &al3608_info.offset_bit_position[37]},
    {AL3608_POS, AL3608_ID, 0x6030, 0x10, &al3608_info.offset[38], &al3608_info.offset_bit_position[38]},
    {AL3608_POS, AL3608_ID, 0x6030, 0x11, &al3608_info.offset[39]}, // Value 4
    // Channel 5
    {AL3608_POS, AL3608_ID, 0x6040, 0x01, &al3608_info.offset[40], &al3608_info.offset_bit_position[40]},
    {AL3608_POS, AL3608_ID, 0x6040, 0x02, &al3608_info.offset[41], &al3608_info.offset_bit_position[41]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6040, 0x07, &al3608_info.offset[43], &al3608_info.offset_bit_position[43]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6040, 0x0f, &al3608_info.offset[45], &al3608_info.offset_bit_position[45]},
    {AL3608_POS, AL3608_ID, 0x6040, 0x10, &al3608_info.offset[46], &al3608_info.offset_bit_position[46]},
    {AL3608_POS, AL3608_ID, 0x6040, 0x11, &al3608_info.offset[47]}, // Value 5
    // Channel 6
    {AL3608_POS, AL3608_ID, 0x6050, 0x01, &al3608_info.offset[48], &al3608_info.offset_bit_position[48]},
    {AL3608_POS, AL3608_ID, 0x6050, 0x02, &al3608_info.offset[49], &al3608_info.offset_bit_position[49]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6050, 0x07, &al3608_info.offset[51], &al3608_info.offset_bit_position[51]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6050, 0x0f, &al3608_info.offset[53], &al3608_info.offset_bit_position[53]},
    {AL3608_POS, AL3608_ID, 0x6050, 0x10, &al3608_info.offset[54], &al3608_info.offset_bit_position[54]},
    {AL3608_POS, AL3608_ID, 0x6050, 0x11, &al3608_info.offset[55]}, // Value 6
    // Channel 7
    {AL3608_POS, AL3608_ID, 0x6060, 0x01, &al3608_info.offset[56], &al3608_info.offset_bit_position[56]},
    {AL3608_POS, AL3608_ID, 0x6060, 0x02, &al3608_info.offset[57], &al3608_info.offset_bit_position[57]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6060, 0x07, &al3608_info.offset[59], &al3608_info.offset_bit_position[59]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6060, 0x0f, &al3608_info.offset[61], &al3608_info.offset_bit_position[61]},
    {AL3608_POS, AL3608_ID, 0x6060, 0x10, &al3608_info.offset[62], &al3608_info.offset_bit_position[62]},
    {AL3608_POS, AL3608_ID, 0x6060, 0x11, &al3608_info.offset[63]}, // Value 7
    // Channel 8
    {AL3608_POS, AL3608_ID, 0x6070, 0x01, &al3608_info.offset[64], &al3608_info.offset_bit_position[64]},
    {AL3608_POS, AL3608_ID, 0x6070, 0x02, &al3608_info.offset[65], &al3608_info.offset_bit_position[65]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6070, 0x07, &al3608_info.offset[67], &al3608_info.offset_bit_position[67]},
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},
    {AL3608_POS, AL3608_ID, 0x6070, 0x0f, &al3608_info.offset[69], &al3608_info.offset_bit_position[69]},
    {AL3608_POS, AL3608_ID, 0x6070, 0x10, &al3608_info.offset[70], &al3608_info.offset_bit_position[70]},
    {AL3608_POS, AL3608_ID, 0x6070, 0x11, &al3608_info.offset[71]}, // Value 8
    {AL3608_POS, AL3608_ID, 0x0000, 0x00, NULL},                    // Gap 16bit
    {}};

/************************* AC1181 PDO/Sync 配置 *************************/
static ec_pdo_entry_info_t ac1181_pdo_entries[] = {
    {0x7000, 0x01, 1},
    {0x7010, 0x01, 1},
    {0x7020, 0x01, 1},
    {0x7030, 0x01, 1},
    {0x7040, 0x01, 1},
    {0x7050, 0x01, 1},
    {0x7060, 0x01, 1},
    {0x7070, 0x01, 1},
    {0x6000, 0x01, 1},
    {0x6010, 0x01, 1},
    {0x6020, 0x01, 1},
    {0x6030, 0x01, 1},
    {0x6040, 0x01, 1},
    {0x6050, 0x01, 1},
    {0x6060, 0x01, 1},
    {0x6070, 0x01, 1},
};
static ec_pdo_info_t ac1181_pdos[] = {
    {0x1600, 8, ac1181_pdo_entries},     // Outputs
    {0x1a00, 8, ac1181_pdo_entries + 8}, // Inputs
};
ec_sync_info_t slave_ac1181_syncs[] = {
    {0, EC_DIR_OUTPUT, 1, ac1181_pdos, EC_WD_ENABLE},
    {1, EC_DIR_INPUT, 1, ac1181_pdos + 1, EC_WD_DISABLE},
    {0xff}};

/************************* AL3608 PDO/Sync 配置 *************************/
static ec_pdo_entry_info_t al3608_pdo_entries[] = {
    {0x7000, 0x01, 16},
    {0x7010, 0x01, 16},
    {0x7020, 0x01, 16},
    {0x7030, 0x01, 16},
    {0x7040, 0x01, 16},
    {0x7050, 0x01, 16},
    {0x7060, 0x01, 16},
    {0x7070, 0x01, 16},
    // Channel 1
    {0x6000, 0x01, 1},
    {0x6000, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6000, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6000, 0x0f, 1},
    {0x6000, 0x10, 1},
    {0x6000, 0x11, 16},
    // Channel 2
    {0x6010, 0x01, 1},
    {0x6010, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6010, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6010, 0x0f, 1},
    {0x6010, 0x10, 1},
    {0x6010, 0x11, 16},
    // Channel 3
    {0x6020, 0x01, 1},
    {0x6020, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6020, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6020, 0x0f, 1},
    {0x6020, 0x10, 1},
    {0x6020, 0x11, 16},
    // Channel 4
    {0x6030, 0x01, 1},
    {0x6030, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6030, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6030, 0x0f, 1},
    {0x6030, 0x10, 1},
    {0x6030, 0x11, 16},
    // Channel 5
    {0x6040, 0x01, 1},
    {0x6040, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6040, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6040, 0x0f, 1},
    {0x6040, 0x10, 1},
    {0x6040, 0x11, 16},
    // Channel 6
    {0x6050, 0x01, 1},
    {0x6050, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6050, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6050, 0x0f, 1},
    {0x6050, 0x10, 1},
    {0x6050, 0x11, 16},
    // Channel 7
    {0x6060, 0x01, 1},
    {0x6060, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6060, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6060, 0x0f, 1},
    {0x6060, 0x10, 1},
    {0x6060, 0x11, 16},
    // Channel 8
    {0x6070, 0x01, 1},
    {0x6070, 0x02, 1},
    {0x0000, 0x00, 4},
    {0x6070, 0x07, 1},
    {0x0000, 0x00, 7},
    {0x6070, 0x0f, 1},
    {0x6070, 0x10, 1},
    {0x6070, 0x11, 16},
    {0x0000, 0x00, 16}, // Gap
};
static ec_pdo_info_t al3608_pdos[] = {
    {0x1600, 1, al3608_pdo_entries + 0},  /* TC Outputs Channel 1 */
    {0x1601, 1, al3608_pdo_entries + 1},  /* TC Outputs Channel 2 */
    {0x1602, 1, al3608_pdo_entries + 2},  /* TC Outputs Channel 3 */
    {0x1603, 1, al3608_pdo_entries + 3},  /* TC Outputs Channel 4 */
    {0x1604, 1, al3608_pdo_entries + 4},  /* TC Outputs Channel 5 */
    {0x1605, 1, al3608_pdo_entries + 5},  /* TC Outputs Channel 6 */
    {0x1606, 1, al3608_pdo_entries + 6},  /* TC Outputs Channel 7 */
    {0x1607, 1, al3608_pdo_entries + 7},  /* TC Outputs Channel 8 */
    {0x1a00, 8, al3608_pdo_entries + 8},  /* TC Channel 1 */
    {0x1a01, 8, al3608_pdo_entries + 16}, /* TC Channel 2 */
    {0x1a02, 8, al3608_pdo_entries + 24}, /* TC Channel 3 */
    {0x1a03, 8, al3608_pdo_entries + 32}, /* TC Channel 4 */
    {0x1a04, 8, al3608_pdo_entries + 40}, /* TC Channel 5 */
    {0x1a05, 8, al3608_pdo_entries + 48}, /* TC Channel 6 */
    {0x1a06, 8, al3608_pdo_entries + 56}, /* TC Channel 7 */
    {0x1a07, 9, al3608_pdo_entries + 64}, /* TC Channel 8 */
};

ec_sync_info_t slave_ac3608_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 8, al3608_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 8, al3608_pdos + 8, EC_WD_DISABLE},
    {0xff}};

/************************* 状态检查函数 *************************/
void check_master_state(void)
{
    ec_master_state_t ms;
    ecrt_master_state(master, &ms);
    if (ms.slaves_responding != master_state.slaves_responding)
        printf("found %u slave(s).\n", ms.slaves_responding);
    if (ms.al_states != master_state.al_states)
        printf("AL states: 0x%02X.\n", ms.al_states);
    if (ms.link_up != master_state.link_up)
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    master_state = ms;
}

void check_slave_states_ac1181(ac1181_info_t *attr)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(attr->sc_, &s);
    if (s.al_state != attr->sc_state_.al_state)
        printf("AC1181 State 0x%02X.\n", s.al_state);
    attr->sc_state_ = s;
}

void check_slave_states_al3608(al3608_info_t *attr)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(attr->sc_, &s);
    if (s.al_state != attr->sc_state_.al_state)
        printf("AL3608 State 0x%02X.\n", s.al_state);
    attr->sc_state_ = s;
}

/************************* 采集与输出 *************************/
void cyclic_read_ac1181(ac1181_info_t *attr)
{
    for (int i = 0; i < 8; ++i)
        attr->data.input[i] = EC_READ_U8(attr->domainpd + attr->offset.input[i]);
}
void cyclic_write_ac1181(ac1181_info_t *attr)
{
    for (int i = 0; i < 8; ++i)
        EC_WRITE_U8(attr->domainpd + attr->offset.output[i], attr->data.output[i]);
}
void cyclic_process_ac1181(ac1181_info_t *attr)
{
    // 示例：输出等于输入
    for (int i = 0; i < 8; ++i)
        attr->data.output[i] = attr->data.input[i];
}
void printInfo_ac1181(ac1181_info_t *attr)
{
    // printf("AC1181输入: ");
    // for (int i = 0; i < 8; ++i)
    //     printf("%d ", attr->data.input[i]);
    // printf(" | 输出: ");
    // for (int i = 0; i < 8; ++i)
    //     printf("%d ", attr->data.output[i]);
    // printf("\n");
}

void cyclic_read_al3608(al3608_info_t *attr)
{
    // 只采集Value 1~8
    // printf("AL3608模拟量采集偏移:attr->offset[15]:%d\n",attr->offset[15]);

    attr->data.input[0] = EC_READ_U16(attr->domainpd + attr->offset[15]);
    attr->data.input[1] = EC_READ_U16(attr->domainpd + attr->offset[23]);
    attr->data.input[2] = EC_READ_U16(attr->domainpd + attr->offset[31]);
    attr->data.input[3] = EC_READ_U16(attr->domainpd + attr->offset[39]);
    attr->data.input[4] = EC_READ_U16(attr->domainpd + attr->offset[47]);
    attr->data.input[5] = EC_READ_U16(attr->domainpd + attr->offset[55]);
    attr->data.input[6] = EC_READ_U16(attr->domainpd + attr->offset[63]);
    attr->data.input[7] = EC_READ_U16(attr->domainpd + attr->offset[71]);
}
void printInfo_al3608(al3608_info_t *attr)
{
    printf("AL3608模拟量: ");
    for (int i = 0; i < 8; ++i)
        printf("CH%d=%u ", i + 1, attr->data.input[i]);
    printf("\n");
}

/************************* 线程与主流程 *************************/
void stack_prefault(void)
{
    unsigned char dummy[MAX_SAFE_STACK];
    memset(dummy, 0, MAX_SAFE_STACK);
}

void *rt_thread(void *arg)
{
    struct timespec wakeup_time;
    clock_gettime(CLOCK_TO_USE, &wakeup_time);
    wakeup_time.tv_sec += 0;
    wakeup_time.tv_nsec = PERIOD_NS;
    while (1)
    {
        int32_t ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeup_time, NULL);
        if (ret)
        {
            fprintf(stderr, "clock_nanosleep(): %s\n", strerror(ret));
            break;
        }
        ecrt_master_application_time(master, TIMESPEC2NS(wakeup_time));
        ecrt_master_receive(master);
        ecrt_domain_process(domain);

        check_master_state();
        check_slave_states_ac1181(&ac1181_info);
        check_slave_states_al3608(&al3608_info);

        cyclic_read_ac1181(&ac1181_info);
        cyclic_read_al3608(&al3608_info);

        cyclic_process_ac1181(&ac1181_info);

        printInfo_ac1181(&ac1181_info);
        printInfo_al3608(&al3608_info);

        cyclic_write_ac1181(&ac1181_info);

        ecrt_master_sync_slave_clocks(master);
        ecrt_domain_queue(domain);
        ecrt_master_send(master);

        wakeup_time.tv_nsec += PERIOD_NS;
        while (wakeup_time.tv_nsec >= NSEC_PER_SEC)
        {
            wakeup_time.tv_nsec -= NSEC_PER_SEC;
            wakeup_time.tv_sec++;
        }
    }
    return NULL;
}

void signal_handler(int signal)
{
    if (signal == SIGINT)
    {
        printf("\n捕获到Ctrl+C信号，程序即将退出。\n");
        ecrt_master_reset(master);
        ecrt_release_master(master);
        exit(0);
    }
}

int main(void)
{
    printf("Starting simple IO example222222\n");
    signal(SIGINT, signal_handler);

    master = ecrt_request_master(0);
    if (!master)
        return -1;

    domain = ecrt_master_create_domain(master);
    if (!domain)
        return -1;

    ec_slave_config_t *ac1181_slave_config = ecrt_master_slave_config(master, AC1181_POS, AC1181_ID);
    if (!ac1181_slave_config)
    {
        fprintf(stderr, "Failed to get AC1181 slave configuration.\n");
        return -1;
    }
    ec_slave_config_t *al3608_slave_config = ecrt_master_slave_config(master, AL3608_POS, AL3608_ID);
    if (!al3608_slave_config)
    {
        fprintf(stderr, "Failed to get AL3608 slave configuration.\n");
        return -1;
    }

    // ====== 增加DC配置 ======
    // ecrt_slave_config_dc(ac1181_slave_config, 0x0300, PERIOD_NS,
    //                      PERIOD_NS / 2, 0, 0);
    // ecrt_slave_config_dc(al3608_slave_config, 0x0300, PERIOD_NS,
    //                      PERIOD_NS / 2, 0, 0);
    // if (ecrt_master_select_reference_clock(master,
    //                                        ac1181_slave_config))
    // {
    //     printf("ecrt_master_select_reference_clock failed !\r\n");
    //     return -1;
    // }

    printf("Configuring PDOs...\n");
    if (ecrt_slave_config_pdos(ac1181_slave_config, EC_END, slave_ac1181_syncs))
    {
        fprintf(stderr, "Failed to configure AC1181 PDOs.\n");
        return -1;
    }
    if (ecrt_slave_config_pdos(al3608_slave_config, EC_END, slave_ac3608_syncs))
    {
        fprintf(stderr, "Failed to configure AL3608 PDOs.\n");
        return -1;
    }

    if (ecrt_domain_reg_pdo_entry_list(domain, domain_regs))
    {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    ac1181_info.domain = domain;
    al3608_info.domain = domain;
    ac1181_info.sc_ = ac1181_slave_config;
    al3608_info.sc_ = al3608_slave_config;

    printf("Activating master...\n");
    if (ecrt_master_activate(master))
        return -1;

    if (!(ac1181_info.domainpd = ecrt_domain_data(ac1181_info.domain)))
        return -1;
    if (!(al3608_info.domainpd = ecrt_domain_data(al3608_info.domain)))
        return -1;

    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    printf("主线程优先级设置为: 最高实时优先级 (SCHED_FIFO)\n");
    if (sched_setscheduler(0, SCHED_FIFO, &param) == -1)
    {
        perror("sched_setscheduler failed");
    }

    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1)
    {
        fprintf(stderr, "Warning: Failed to lock memory: %s\n", strerror(errno));
    }
    stack_prefault();

    printf("Starting RT task with dt=%u ns.\n", PERIOD_NS);

    pthread_t rt_tid;
    if (pthread_create(&rt_tid, NULL, rt_thread, NULL) != 0)
    {
        fprintf(stderr, "无法创建实时线程\n");
        return EXIT_FAILURE;
    }

    // 绑定主线程到CPU核心0
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(0, &cpuset);
    if (sched_setaffinity(0, sizeof(cpu_set_t), &cpuset) != 0)
    {
        perror("绑定主线程到CPU 0失败");
    }
    else
    {
        printf("主线程已绑定到CPU 0\n");
    }

    pthread_join(rt_tid, NULL);
    return 0;
}